/*倒立摆系统*/
#ifndef PENDULUM_H
#define PENDULUM_H
#include "simucpp.hpp"
using namespace simucpp;
using namespace std;

/**********************
短公式定义
**********************/
class Funcn1: public UserFunc {
public:
    Funcn1(double ipM, double ipm): ipM(ipM), ipm(ipm) {}
    virtual double Function(double u) const{ return 1/(ipM+ipm*sin(u)*sin(u)); }
    double ipM, ipm;
};
class Funcn2: public UserFunc {
public:
    Funcn2(double ipm, double ipl): ipm(ipm), ipl(ipl) {}
    virtual double Function(double *u) const{ return -ipm*ipl*u[0]*u[0]*sin(u[1]); }
    double ipm, ipl;
};
class Funcn3: public UserFunc {  // 5m sin 2θ
public:
    Funcn3(double ipm): ipm(ipm) {}
    virtual double Function(double u) const{ return 5*ipm*sin(u+u); }
    double ipm;
};
class Funcn4: public UserFunc {
public:
    virtual double Function(double *u) const{ return u[0]*(u[1]+u[2]+u[3]); }
};
class Funcn5: public UserFunc {
public:
    Funcn5(double ipl): ipl(ipl) {}
    virtual double Function(double *u) const{ return (u[0]*cos(u[1])+10*sin(u[1]))/ipl; }
    double ipl;
};
class Funcn1a: public UserFunc {
public:
    Funcn1a(double ipM, double ipm1, double ipm2): ipM(ipM), ipm1(ipm1), ipm2(ipm2) {}
    virtual double Function(double *u) const{
        double temp = ipM;
        temp += ipm1*sin(u[0])*sin(u[0]);
        temp += ipm2*sin(u[1])*sin(u[1]);
        return 1/temp;
    }
    double ipM, ipm1, ipm2;
};
class Funcn4a: public UserFunc {
public:
    virtual double Function(double *u) const{ return u[0]*(u[1]+u[2]+u[3]+u[4]+u[5]); }
};


/**********************
简单的单倒立摆
**********************/
class InversePendulum: public PackModule
{
public:
    InversePendulum() {};
    InversePendulum(Simulator *sim) {
        Initialize(sim);
    };
    virtual PUnitModule Get_InputPort(int n) const {
        if (n==0) return mioF;
        return nullptr;
    };
    virtual PUnitModule Get_OutputPort(int n) const {
        if (n==0) return mintx;
        if (n==1) return mintv;
        if (n==2) return minttheta;
        if (n==3) return mintomega;
        return nullptr;
    };
    void Initialize(Simulator *sim){
        SMIntegrator(minttheta, sim);
        SMIntegrator(mintomega, sim);
        SMIntegrator(mintx, sim);
        SMIntegrator(mintv, sim);
        SMFcn(mfcn1, sim);
        SMFcnMISO(mfcn2, sim);
        SMFcn(mfcn3, sim);
        SMFcnMISO(mfcn4, sim);
        SMFcnMISO(mfcn5, sim);
        SMConnector(mioF, sim);
        sim->connect(mfcn5, mintomega); // ω'=n5
        sim->connect(mintomega, minttheta);  // θ'=ω
        sim->connect(mfcn4, mintv);  // v'=n4
        sim->connect(mintv, mintx);  // x'=v
        sim->connect(minttheta, mfcn1);
        sim->connect(mintomega, mfcn2);
        sim->connect(minttheta, mfcn2);
        sim->connect(minttheta, mfcn3);
        sim->connect(mfcn1, mfcn4);
        sim->connect(mioF, mfcn4);
        sim->connect(mfcn2, mfcn4);
        sim->connect(mfcn3, mfcn4);
        sim->connect(mfcn4, mfcn5);
        sim->connect(minttheta, mfcn5);
        n1 = new Funcn1(ipM, ipm);
        n2 = new Funcn2(ipm, ipl);
        n3 = new Funcn3(ipm);
        n4 = new Funcn4;
        n5 = new Funcn5(ipl);
        mfcn1->Set_Function(n1);
        mfcn2->Set_Function(n2);
        mfcn3->Set_Function(n3);
        mfcn4->Set_Function(n4);
        mfcn5->Set_Function(n5);
        minttheta->Set_InitialValue(0.2);
    }
private:
    double ipM=0.5, ipm=0.2, ipl=0.3;
    MIntegrator *minttheta=nullptr;  // θ
    MIntegrator *mintomega=nullptr;  // ω
    MIntegrator *mintx=nullptr;  // x
    MIntegrator *mintv=nullptr;  // v
    MFcn *mfcn1=nullptr;  // 式n1
    MFcnMISO *mfcn2=nullptr;  // 式n2
    MFcn *mfcn3=nullptr;  // 式n3
    MFcnMISO *mfcn4=nullptr;  // 式n4
    MFcnMISO *mfcn5=nullptr;  // 式n5
    MConnector *mioF=nullptr;  // 输入的力F
    UserFunc *n1, *n2, *n3, *n4, *n5;
};


/**********************
<Robust Adaptive Control>中的双倒立摆
**********************/
class InversePendulumTwo: public PackModule
{
public:
    InversePendulumTwo() {};
    InversePendulumTwo(Simulator *sim) {
        Initialize(sim);
    };
    virtual PUnitModule Get_InputPort(int n) const {
        if (n==0) return mioF;
        return nullptr;
    };
    virtual PUnitModule Get_OutputPort(int n) const {
        if (n==0) return minttheta1;
        if (n==1) return mintomega1;
        if (n==2) return minttheta2;
        if (n==3) return mintomega2;
        if (n==4) return mintv;
        if (n==5) return mintx;
        return nullptr;
    };
    void Initialize(Simulator *sim){
        n1a = new Funcn1a(ipM, ipm1, ipm2);
        n4a = new Funcn4a;
        n12 = new Funcn2(ipm1, ipl1);
        n13 = new Funcn3(ipm1);
        n15 = new Funcn5(ipl1);
        n22 = new Funcn2(ipm2, ipl2);
        n23 = new Funcn3(ipm2);
        n25 = new Funcn5(ipl2);
        SMIntegrator(minttheta1, sim);
        SMIntegrator(mintomega1, sim);
        SMIntegrator(minttheta2, sim);
        SMIntegrator(mintomega2, sim);
        SMIntegrator(mintx, sim);
        SMIntegrator(mintv, sim);
        SMFcnMISO(mfcn1a, sim);
        SMFcnMISO(mfcn4a, sim);
        SMFcnMISO(mfcn12, sim);
        SMFcn(mfcn13, sim);
        SMFcnMISO(mfcn15, sim);
        SMFcnMISO(mfcn22, sim);
        SMFcn(mfcn23, sim);
        SMFcnMISO(mfcn25, sim);
        SMConnector(mioF, sim);
        sim->connect(mfcn15, mintomega1); // ω'=n5
        sim->connect(mintomega1, minttheta1);  // θ'=ω
        sim->connect(mfcn25, mintomega2); // ω'=n5
        sim->connect(mintomega2, minttheta2);  // θ'=ω
        sim->connect(mfcn4a, mintv);  // v'=n4
        sim->connect(mintv, mintx);  // x'=v
        sim->connect(minttheta1, mfcn1a);
        sim->connect(minttheta2, mfcn1a);
        sim->connect(mintomega1, mfcn12);
        sim->connect(minttheta1, mfcn12);
        sim->connect(mintomega2, mfcn22);
        sim->connect(minttheta2, mfcn22);
        sim->connect(minttheta1, mfcn13);
        sim->connect(minttheta2, mfcn23);
        sim->connect(mfcn1a, mfcn4a);
        sim->connect(mioF, mfcn4a);
        sim->connect(mfcn12, mfcn4a);
        sim->connect(mfcn13, mfcn4a);
        sim->connect(mfcn22, mfcn4a);
        sim->connect(mfcn23, mfcn4a);
        sim->connect(mfcn4a, mfcn15);
        sim->connect(mfcn4a, mfcn25);
        sim->connect(minttheta1, mfcn15);
        sim->connect(minttheta2, mfcn25);
        mfcn1a->Set_Function(n1a);
        mfcn4a->Set_Function(n4a);
        mfcn12->Set_Function(n12);
        mfcn13->Set_Function(n13);
        mfcn15->Set_Function(n15);
        mfcn22->Set_Function(n22);
        mfcn23->Set_Function(n23);
        mfcn25->Set_Function(n25);
        minttheta1->Set_InitialValue(0.2);
        minttheta2->Set_InitialValue(0.1);
    }
private:
    double ipM=10, ipm1=1, ipl1=1, ipm2=1, ipl2=2;
    MIntegrator *minttheta1=nullptr;  // θ1
    MIntegrator *mintomega1=nullptr;  // ω1
    MIntegrator *minttheta2=nullptr;  // θ2
    MIntegrator *mintomega2=nullptr;  // ω2
    MIntegrator *mintx=nullptr;  // x
    MIntegrator *mintv=nullptr;  // v
    MFcnMISO *mfcn1a=nullptr;
    MFcnMISO *mfcn4a=nullptr;
    MFcnMISO *mfcn12=nullptr;
    MFcn *mfcn13=nullptr;
    MFcnMISO *mfcn15=nullptr;
    MFcnMISO *mfcn22=nullptr;
    MFcn *mfcn23=nullptr;
    MFcnMISO *mfcn25=nullptr;
    MConnector *mioF=nullptr;
    UserFunc *n1a, *n4a, *n12, *n13, *n15, *n22, *n23, *n25;
};

#endif
